#include <math.h>
#include <fstream>
#include <iostream>

#include <route_planner/Astar.h>

void Astar::InitAstar(const std::string &filename)
{
/*    pointVec points;
    std::ifstream in(filename);
    while(in.eof())
    {
        double x,y;
	      in>>x>>y;
        points.push_back({x,y});
    }
    waypoints(points);
    in.close();
*/
}


double Astar::calcG(Point *temp_start, Point *point)
{
	double extraG = sqrt(pow((point->x - temp_start->x),2) + pow((point->y - temp_start->y),2));
	double parentG = point->parent == NULL ? 0 : point->parent->G; //如果是初始节点，则其父节点是空
	return parentG + extraG;
}

double Astar::calcH(Point *point, Point *end)
{
	//欧几里何距离
	return sqrt(pow((end->x - point->x),2) + pow((end->y - point->y),2));
}

double Astar::calcF(Point *point)
{
	return point->G + point->H;
}

Point *Astar::getLeastFpoint()
{
	if (!openList.empty())
	{
		auto resPoint = openList.front();
		for (auto &point : openList)
		if (point->F<resPoint->F)
			resPoint = point;
		return resPoint;
	}
	return NULL;
}

Point *Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
	openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离
	while (!openList.empty())
	{
		auto curPoint = getLeastFpoint(); //找到F值最小的点
		//std::cout<<curPoint->x<<" "<<curPoint->y<<std::endl;
		openList.remove(curPoint); //从开启列表中删除
		closeList.push_back(curPoint); //放到关闭列表
		//1,找到当前周围八个格中可以通过的格子
		auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
		for (auto &target : surroundPoints)
		{
			//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H
			if (!isInList(openList, target))
			{
				target->parent = curPoint;

				target->G = calcG(curPoint, target);
				target->H = calcH(target, &endPoint);
				target->F = calcF(target);

				openList.push_back(target);
			}
			//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
			else
			{
				int tempG = calcG(curPoint, target);
				if (tempG<target->G)
				{
					target->parent = curPoint;

					target->G = tempG;
					target->F = calcF(target);
				}
			}
			Point *resPoint = isInList(openList, &endPoint);
			if (resPoint)
				return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
		}
	}

	return NULL;
}

std::list<Point *> Astar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
	Point *result = findPath(startPoint, endPoint, isIgnoreCorner);
	std::list<Point *> path;
	//返回路径，如果没找到路径，返回空链表
	while (result)
	{
		path.push_front(result);
		result = result->parent;
	}

	// 清空临时开闭列表，防止重复执行GetPath导致结果异常
	openList.clear();
	closeList.clear();

	return path;
}

Point *Astar::isInList(const std::list<Point *> &list, const Point *point) const
{
	//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
	for (auto p : list)
	if (p->x == point->x&&p->y == point->y)
		return p;
	return NULL;
}

bool Astar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const
{
  if (isInList(closeList, target))
  {
    return 0;
  }
  return 1;
/*	if (target->x<0 || target->x>maze.size() - 1
		|| target->y<0 || target->y>maze[0].size() - 1
		|| maze[target->x][target->y] == 1
		|| target->x == point->x&&target->y == point->y
		|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
		return false;
	else
	{
		if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以
			return true;
		else
		{
			//斜对角要判断是否绊住
			if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)
				return true;
			else
				return isIgnoreCorner;
		}
	}
*/
}

std::vector<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const
{
	std::vector<Point *> surroundPoints;

  pointVec kdsurroundingpoints;
  point_t thispoint;
  thispoint.push_back(point->x);
  thispoint.push_back(point->y);

  kdsurroundingpoints = waypoints.neighborhood_points(thispoint,searching_rad);

  for (auto p : kdsurroundingpoints)
  {
    Point *sp = new Point(p[0],p[1]);
    if (isCanreach(point, sp, isIgnoreCorner))
    surroundPoints.push_back(sp);
  }
	//for (int x = point->x - 1; x <= point->x + 1; x++)
	//for (int y = point->y - 1; y <= point->y + 1; y++)
	//if (isCanreach(point, new Point(x, y), isIgnoreCorner))
	//surroundPoints.push_back(new Point(x, y));
	return surroundPoints;
}
